#
# Project:    RoboBerDuino
# Author:       Marco Tili
#
# Platform:    Raspberry Pi Model B
#
# Filename:            TBD
# Version:           00.01
#
# Description:
#      TBD
#
#
#
# Changelog:
#       2013-03-22   | M.Tili
#                    | First Issue
#       2013-04-16   | M.Tili
#                    | Fix for byte mode in tx/rx
#                    | Queues for incoming and outgoing are int type (code,[body])s
#       2013-04-17   | M.Tili
#                    | Minor fixes
#
#
#


from select import select
import serial
import sys
import os
from serial.tools import list_ports
import Queue
import time


class TaskSerial():
    ser       = 0
    port      = "loop://logging=info"
    baudrate  = 9600
    bytesize  = serial.EIGHTBITS
    parity    = serial.PARITY_NONE
    stopbits  = serial.STOPBITS_ONE
    timeout   = None
    xonxoff   = False
    rtscts    = False,
    writeTimeout = None,
    dsrdtr    = False,
    interCharTimeout = None
    rx_buff   = None
    tx_buff   = None
    rx_q      = None
    tx_q      = None
    rx_fsm    = 0

    def __init__(self,rx_q,tx_q):
        self.rx_q = rx_q
        self.tx_q = tx_q
        self.port             = None
        self.baudrate         = 9600
        self.bytesize         = serial.EIGHTBITS
        self.parity           = serial.PARITY_NONE
        self.stopbits         = serial.STOPBITS_ONE
        self.timeout          = 0
        self.xonxoff          = False
        self.rtscts           = False
        self.writeTimeout     = None
        self.dsrdtr           = False
        self.interCharTimeout = None

    def list_serial_ports(self):
        # Windows
        if os.name == 'nt':
            # Scan for available ports.
            available = []
            for i in range(256):
                try:
                    s = serial.Serial(i)
                    #s = Serial(1)
                    available.append('COM'+str(i + 1))
                    s.close()
                except serial.SerialException:
                #except SerialException:
                    pass
            return available
        else:
            # Mac / Linux
            return [port[0] for port in list_ports.comports()]

    def Open(self,port,baudrate,timeout):
        ret = False
        self.port     = port
        self.baudrate = baudrate
        self.timeout  = timeout
        try:
            self.ser = serial.Serial( self.port, self.baudrate,
                                      self.bytesize, self.parity, self.stopbits,
                                      self.timeout, self.xonxoff, self.rtscts,
                                      self.writeTimeout, self.dsrdtr,
                                      self.interCharTimeout )
            ret = self.IsOpen()
        except:
            ret = False
        return ret

    def CloseConnection(self):
        self.ser.close()
        return self.ser.isOpen()

    def IsOpen(self):
        return self.ser.isOpen()

    def IsConnected(self):
        return self.ser.getCD()
    
    def IsConnAlive(self):
        ret = False
        try:
            ret = self.ser.getCD()
        except:
            ret = False
        return ret

    def DoRX(self):
        ret = False
        data = None
        while self.ser.inWaiting()>0:
            ret = True
            data = self.ser.read(self.ser.inWaiting())
        return ret,data

    def DoTX(self,data):
        for b in data:
            self.ser.write(b)
        print "DEBUG: TX COMPLETE!", data


    def UpdateRX(self):
        ret = False
        isRx,buff = self.DoRX()
        if isRx!=False:
            self.UnpackMessage(map(ord,buff)) # type(buff) is str
            ret = True
        return ret

    def UpdateTX(self):
        ret = False
        if self.tx_q:
            if not self.tx_q.empty:
                tmp_code,tmp_body = tx_q.get()
                self.DoTX(self.PackMessage(tmp_code,tmp_body))
                ret = True
        return ret

    def PackMessage(self,code,body):
        frame =  ['<','$','>',chr(code)] 
        frame += [chr(i) for i in body] + [chr(170), chr(187)]
        return frame

    def UnpackMessage(self,frame): # type(frame) and type(buff_tx) must be int 
        rx_complete = False
        print "DEBUG: rx_fsm [IN] =", self.rx_fsm        
        for byte in frame:
            print "DEBUG: byte =", byte
            if self.rx_fsm==0:
                self.rx_buff=[]
                if byte==ord('<'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 1 -", self.rx_buff
                    self.rx_fsm = 1
            elif self.rx_fsm==1:
                if byte==ord('$'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 2 -", self.rx_buff
                    self.rx_fsm = 2
                else: #elif byte!=ord('<')
                    self.rx_fsm  = 0
                    self.rx_buff = []
            elif self.rx_fsm==2:
                if byte==ord('>'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 3 -", self.rx_buff
                    self.rx_fsm = 3
                else:
                    self.rx_fsm  = 0
                    self.rx_buff = []
            elif self.rx_fsm==3:
                #header
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=6:
                    length = self.rx_buff[4]
                    length = (length<<8) +self.rx_buff[5]
                    if length!=0:
                        self.rx_fsm=4
                    else:
                        self.rx_fsm=5
            elif self.rx_fsm==4:
                #body
                length = self.rx_buff[4]
                length = (length<<8) +self.rx_buff[5]
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=(6+length):
                    self.rx_fsm = 5
            elif self.rx_fsm==5:
                #footer
                length=self.rx_buff[4]
                length= (length<<8) +self.rx_buff[5]
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=(6+length+2):
                    # FIXME check CRC
                    rx_complete = True
                    print "DEBUG: RX COMPLETE!", self.rx_buff
                    if self.rx_q:
                        # put : [code,body]
                        tmp_code = self.rx_buff[HPOS_CODE]
                        tmp_body = [b for b in self.rx_buff[HPOS_DATASTART:HPOS_DATASTART+length]]
                        self.rx_q.put([tmp_code,tmp_body])
                    self.rx_fsm = 0
        return rx_complete
  
if __name__ == "__main__":        
    x = TaskSerial(None,None)
    l = x.list_serial_ports()
    print "Available COM ports:\n",l
    try:
        p = raw_input("Enter port (CTRL+C to quit): ")
    except KeyboardInterrupt:
        exit(0)
    if p in l:
        try:
            print "Opening...state=",x.Open(p,9600,1)
            w = raw_input("Type something to send (Enter to skip): ")
            if w:
                print w
                x.DoTX(w)
                print "Data sent. Waiting for answer (CTRL+C to skip)."
                run = True
                while run:
                    try:
                        a1,a2 = x.DoRX()
                        if a1:
                            print "Received:",a2
                        time.sleep(0.1)
                    except KeyboardInterrupt:
                        run = False
            else:
                print "Skip writing."
            print "Closing...state=",x.CloseConnection()
        except:
            print "Error!"
            exit(0)
    else:
        print "Wrong port"
    


